#include "timers.h"
#include "interrupt_controller.h"
#include "os_mem.h"
#include "os_io.h"

static void (*user_timer_event_handler)(void *, void *) = NULL;

uint32_t running_bucket = 0;

#if 0
/*
* We know that a timer won't be requested with
* buckets < MIN_BUCKETS_INTERVAL
* So unless already filled, we are sure that the
* next MIN_BUCKETS_INTERVAL amount of buckets won't
* get filled in the running pass. So we set 
* free_buckets_remaining to this (or lesser)
* value and keep decrementing on every timer hit, so 
* that we don't have to peep into the buckets
* on every timer hit.
*/
static uint32_t free_buckets_remaining = 0;
#endif

static bucket_t *wheels[] = {NULL, NULL, NULL};
static const wheel_info_t wheels_info[] = 
{
  {(NUM_BUCKETS_IN_WHEEL0 - 1), 0},  // wheel 0, 64 buckets 
  {(NUM_BUCKETS_IN_WHEEL1 - 1), (NUM_BITS_IN_WHEEL1)}, \
  {(NUM_BUCKETS_IN_WHEEL2 - 1), (NUM_BITS_IN_WHEEL1 + NUM_BITS_IN_WHEEL2)}
};

void reset_processor(uint32_t data)
{
    // WDOG timer is pre-active
    // Activate it and it should reset the processor in 2 ticks

    printf("Forcing WDOG timeout to reset the processor\n");

    TIM_WDTCR(WDOG_TIMER) = 0xA5C64000;
    TIM_WDTCR(WDOG_TIMER) = 0xDA7E4000;

    // Timer is now active. 
    // Now writing anything apart from 0xA5C6,DA7E
    // should cause a timeout => processor reset
    TIM_WDTCR(WDOG_TIMER) = 0x4000;

    // should not reach here
    printf("WDOG time-out failed\n");
    return;  
}

static void setup_wdog(void)
{
    TIM_TGCR(WDOG_TIMER) = 0x00;  // reset the timer
    TIM_TGCR(WDOG_TIMER) = 0x0B;  // wdog mode

    // Program the counts
    TIM_PRD12(WDOG_TIMER) = 2;  // Reset in 2 ticks when activated
    TIM_PRD34(WDOG_TIMER) = 0;
    TIM_TIM12(WDOG_TIMER) = 0;
    TIM_TIM12(WDOG_TIMER) = 0;

    // Enable WDOG timer
    TIM_WDTCR(WDOG_TIMER) = 0x4000;

    // WDOG timer is now pre-active
    // We shall activate it when we want to explicitely reset the processor

    return;
} 

err_t timer_init(void (*expiry_handler)(void *, void *), uint32_t *tim_entry_size)
{
    bucket_t *bucket_array = NULL;
    uint16_t i = 0;

    if (!expiry_handler || !tim_entry_size) { 
        return ERR_INVALID;
    }
    
    TIM_TGCR(USER_TIMER) = 0x00; // reset timer, enable 64 bit mode
    TIM_TGCR(USER_TIMER) = 0x03; // remove from reset
   
    
    TIM_PRD12(USER_TIMER) = NUM_TICKS_PER_BUCKET;
    TIM_PRD34(USER_TIMER) = 0x00;

    TIM_TIM12(USER_TIMER) = 0x00;   // reset initial count
    TIM_TIM34(USER_TIMER) = 0x00;   

    enable_interrupt(AINTC_USER_TIMER_INTERRUPT);  // enable interrupt at INT controller
    //TIM_INTCTLSTAT(USER_TIMER) = 0x00010001;    // enable interrupts at timer unit
    user_timer_event_handler = expiry_handler;

    running_bucket = 0;
    bucket_array = (bucket_t *)calloc(TOTAL_BUCKETS_IN_WHEELS, sizeof(bucket_t));
    if (!bucket_array) {
        printf("Timer wheel allocation fail!!\n");
	return ERR_NO_MEM;
    }

    for (i = 0; i < TOTAL_BUCKETS_IN_WHEELS; i++) {
        bucket_array[i].sentinel.prev = &bucket_array[i].sentinel;
    }

    wheels[0] = bucket_array;
    wheels[1] = bucket_array + NUM_BUCKETS_IN_WHEEL0;
    wheels[2] = wheels[1] + NUM_BUCKETS_IN_WHEEL1;

    TIM_TCR(USER_TIMER) = 0x80;		// start 64 bit continous timer

    *tim_entry_size = sizeof(tim_entry_t);

    /*
    * Also setup wdog timer.
    * Currently it is disabled at run-time and is used
    * only when the software wants to reset the processor
    */
    setup_wdog();
    return ERR_SUCCESS;
}


err_t cancel_submitted_timer(void *handle)
{
    tim_entry_t *tim_entry = (tim_entry_t *)handle;
    
    if (!tim_entry || !tim_entry->prev) {
        return ERR_INVALID;
    }

    // Remove timer entry from its bucket
    tim_entry->prev->next = tim_entry->next;

    if (tim_entry->next) {
        tim_entry->next->prev = tim_entry->prev;
    } else {
        tim_entry->sentinel->prev = tim_entry->prev;
    }

    tim_entry->next = tim_entry->prev = NULL;

    //printf("%s: Timer cancelled: 0x%x\n", __FILE__, tim_entry);

    return ERR_SUCCESS;
}


static inline err_t add_tim_entry_to_wheels(tim_entry_t *tim_entry,
            uint32_t num_buckets)
{
    uint32_t buckets_covered = 0;
    uint32_t wheel_index = 0;
    uint8_t wheel = 0;
    const wheel_info_t *w_info;
    bucket_t *target_bucket = NULL;

    
    // First check if the interval > supported wheels size
    if ((num_buckets >>NUM_WHEELS_BITS))
    {
        /* Say we are at A.B.C. Add timer entry to (A-1).0.0
         buckets covered = 
	 (MAX-C) => A.(B+1).0    (MAX = 64)
	 + (MAX-(B+1))*w0_max => A+1.0.0
	 + (MAX-1)*w1_max*w0_max */
	printf("num_buckets > NUM_WHEEL_BITS\n");
	buckets_covered = NUM_BUCKETS_IN_WHEEL0 - CUR_W0(running_bucket);
	buckets_covered += (NUM_BUCKETS_IN_WHEEL1 - (CUR_W1(running_bucket) + 1)) <<NUM_BITS_IN_WHEEL0;
	buckets_covered += (((NUM_BUCKETS_IN_WHEEL2 - 1) << NUM_BITS_IN_WHEEL1) <<NUM_BITS_IN_WHEEL0);
	target_bucket = &wheels[2][(CUR_W2(running_bucket) - 1) & (NUM_BUCKETS_IN_WHEEL2 - 1)];
    } else {
       /* Time interval well within some wheel */
       wheel = 2;
       do
       {
	   w_info = &wheels_info[wheel];

           buckets_covered = (((num_buckets >> w_info->shift) & w_info->mask) << w_info->shift);
	   if (((num_buckets >> w_info->shift) & w_info->mask) == 
	       ((running_bucket >> w_info->shift) & w_info->mask)) {
	       // Running bucket has already passed the current bucket in this wheel
	       num_buckets -= buckets_covered;
	       buckets_covered = 0;
	   }

	   if (buckets_covered)
	   {
	       wheel_index = ((num_buckets >> w_info->shift) & w_info->mask);
	       target_bucket = &wheels[wheel][wheel_index];
	      // printf("Running bucket:0x%x, wheel:0x%x, wheel_index:0x%x, buckets_covered:0x%x\n",
	       //        running_bucket, wheel, wheel_index, buckets_covered);
	   }
	   wheel--;
       }while(buckets_covered == 0);
    }

    tim_entry->buckets_remaining = num_buckets - buckets_covered;
    
    //printf("tim_entry: 0x%x, Absolute bucket:0x%x, running_bucket:0x%x, wheel/index: 0x%x/0x%x\n", tim_entry, num_buckets, running_bucket, ((wheel + 1) & 0xFF), wheel_index);

    /* add to timer list in bucket */
    tim_entry->next = NULL;
    tim_entry->prev = target_bucket->sentinel.prev;
    target_bucket->sentinel.prev->next = tim_entry;
    target_bucket->sentinel.prev = tim_entry;
    tim_entry->sentinel = &target_bucket->sentinel;

    if (target_bucket->sentinel.next != tim_entry) {
        // multiple entries in same bucket
	//printf("%s: Multiple entries in same bucket\n", __FILE__);
    }


    return ERR_SUCCESS;
}

err_t timer_submit(void *handle, void *tim_data, uint32_t interval_ms)
{
    tim_entry_t *tim_entry = (tim_entry_t *)handle;
    uint32_t num_buckets = 0;

    if (interval_ms < MIN_TIMER_INTERVAL || interval_ms > MAX_TIMER_INTERVAL) {
        return ERR_INVALID;
    }

    num_buckets = NUM_BUCKETS_IN_INTERVAL_MS(interval_ms);

    //tim_entry->init_buckets = num_buckets;
    tim_entry->caller_ctx = (void *)tim_data;

    if (num_buckets > ((0x1L << NUM_WHEELS_BITS) - 1)) {
        num_buckets += running_bucket;  // absolute bucket
    } else {
        num_buckets += running_bucket;
	num_buckets &= ((0x1L << NUM_WHEELS_BITS) - 1);
    }

    return add_tim_entry_to_wheels(tim_entry, num_buckets);
}


void static inline process_timers(tim_entry_t *cur_entry)
{
    tim_entry_t *next_entry = NULL;

    do {
        next_entry = cur_entry->next;
        cur_entry->next = NULL;
	cur_entry->prev = NULL;

        if (cur_entry->buckets_remaining) {
	    add_tim_entry_to_wheels(cur_entry, cur_entry->buckets_remaining);
	} else {
	    user_timer_event_handler(cur_entry, cur_entry->caller_ctx);
	}

	cur_entry = next_entry;
    } while (cur_entry != NULL);

    return;
}

void user_timer_interrupt_handler(void)
{
    bucket_t *target_bucket = NULL;
    uint8_t wheel = 0;
    const wheel_info_t *w_info;
    uint32_t wheel_index = 0;

    //TIM_INTCTLSTAT(USER_TIMER) = 0x00030003;  // clear interrupt in timer reg
    ARM_INTC_SICR = 21;		// Clear interrupt from interrupt controller
    running_bucket++;

#if 0
    if (free_buckets_remaining--) {
        /*
	* We know the bucket is empty.
	* No need to peep into data
	* structures
	*/
        return;
    }
#endif

    wheel = 0;
    do 
    {
        w_info = &wheels_info[wheel];
	wheel_index = ((running_bucket >>w_info->shift) & w_info->mask);
	target_bucket = &wheels[wheel][wheel_index];
	if (target_bucket->sentinel.next) {
	   // printf("Some timer expired\n");
	    process_timers(target_bucket->sentinel.next);
	    target_bucket->sentinel.next = NULL;
	    target_bucket->sentinel.prev = &target_bucket->sentinel;
	}
	wheel++;
    }while (wheel_index == 0 && wheel < 3);

#if 0
    /*
    * Scan upto next MIN_BUCKETS_INTERVAL buckets
    * till we find a filled bucket
    * All the empty buckets found till then cannot
    * be filled in the current wheel, so set
    * free_buckets_remaining to that value to skip
    * peeping into wheels on every timer interrupt
    */
    //free_buckets_remaining = 0;
    i = 0;
    w_info = &wheels_info[0];
    wheel_index = ((running_bucket + 1) & w_info->mask);
    target_bucket = &wheels[0][wheel_index];
    while (i < MIN_BUCKETS_INTERVAL) {
        if (!target_bucket->first) {
	    i++;
	    wheel_index = (wheel_index + 1) & w_info->mask;
	    target_bucket = &wheels[0][wheel_index];
	} else {
	    break;
	}
    }

    // Disabled free-bucket logic.
    //free_buckets_remaining = i;
    //free_buckets_remaining = 0;
    //printf("Next 0x%x buckets free\n", free_buckets_remaining);
#endif
    return;

}
